perm filename CAMERA.SAI[VV,BGB] blob
sn#109201 filedate 1974-11-21 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00025 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00004 00002 BEGIN "CAMERA" DEFINE DEBUG="TRUE" comment TRUE for debugging code
C00007 00003 \ VARIABLES, DEFAULT VALUES
C00011 00004 \ pot limits, error flags, and names - cosqr procedure
C00014 00005 \ MISC. PROCEDURES
C00017 00006 \ compute pot readings for each motor and check errors
C00019 00007 \ READS DATA IN GILL'S FORMAT FOR GIVEN CAM/LENS #
C00024 00008 \ SERVO THE DEFAULT CAMERA USING THE STORED POT VALUES
C00026 00009
C00028 00010 \ Compute the transformation matrices for the camera model
C00031 00011
C00034 00012 \ CREATE UPDATED TRANSFORM
C00037 00013 \ UPDATE THE TRANSFORM FOR THE GIVEN CAMERA
C00040 00014 \ UPDATE THE TRANSFORM (cont.)
C00043 00015 \ UPDATE THE TRANSFORM - process readings
C00049 00016 \ move camera - default camera set to given camera and update
C00052 00017
C00061 00018 \ CALCULATIONS COMMON TO PREDICTION AND CENTERING GIVEN LENS
C00063 00019 \ CENTER CAMERA - sets default camera to given and update
C00071 00020 \ TRANSFORM PREDICTION - set default camera to given camera
C00074 00021 \ PRINT LIMITS FOR GIVEN CAMERA
C00077 00022 \ COORDINATE CONVERSION ROUTINES
C00080 00023 \ PROGRAM TEST ROUTINE
C00082 00024
C00084 00025 \ THIS IS THE MAIN PROGRAM
C00086 ENDMK
C⊗;
BEGIN "CAMERA" DEFINE DEBUG="TRUE"; comment TRUE for debugging code;
REQUIRE "PREAMB.SAI" SOURCE_FILE;
REQUIRE "HELIB[1,3]" LIBRARY;
REQUIRE "SOBMAT[SYS,HE]" LOAD_MODULE;
REQUIRE -1 NEW_ITEMS;
REQUIRE "⊂⊃||" DELIMITERS;
EXTERNAL PROCEDURE CWHEEL(INTEGER CODE);
EXTERNAL PROCEDURE TELL(INTEGER DEVICE);
EXTERNAL PROCEDURE CALLEN;
EXTERNAL PROCEDURE SPWON(INTEGER TIC;REFERENCE INTEGER ADDR);
EXTERNAL PROCEDURE SPWOFF;
EXTERNAL PROCEDURE INVRT(REAL ARRAY A,AI);
ifc debug thenc
FORWARD SIMPLE PROCEDURE CONVRT(REAL A,B;REFERENCE REAL C,D);
EXTERNAL PROCEDURE TVIN;
EXTERNAL INTEGER PROCEDURE GIOWD(INTEGER ARRAY FOO);
endc;
DEFINE \=⊂COMMENT⊃,
KY=⊂684.5⊃, comment zoom constant;
DEG=⊂57.29578⊃, comment radian to degrees conversion;
SAFEX=⊂ ⊃,
CRLF=⊂&'15&'12⊃,
YES=⊂INCHWL="Y"⊃,
RETYPE=⊂OUTSTR(" ... type Y<cr> to retry ")⊃,
OCT(A)=⊂" A="&CVOS(A)⊃,
INT(A)=⊂" A="&CVS(A)⊃,
FLT(A)=⊂" A="&CVF(A)⊃,
SAVE=⊂CAMERR[CAMNUM]←CAMFLG⊃,
ERROR(I)=⊂CAMFLG←CAMFLG LOR (I)⊃,
CAMSET(I)=⊂ IF ¬(COHU≤I≤SIERRA) THEN
BEGIN ERROR(NUMERR);SAVE;RETURN;END ELSE CAMNUM←I;
CAMJOB[CAMNUM] ← JOB⊃;
\ ERROR BITS;
DEFINE PANERR=⊂1⊃, comment PAN OUT OF BOUNDS;
TILERR=⊂2⊃, comment TILT OUT OF BOUNDS;
FOCERR=⊂4⊃, comment FOCUS OUT OF BOUNDS;
ZOMERR=⊂'10⊃, comment ZOOM OUT OF BOUNDS;
IRSERR=⊂'20⊃, comment IRIS OUT OF BOUNDS;
LENERR=⊂'40⊃, comment LENS OUT OF BOUNDS;
CHGERR=⊂'100⊃, comment LENS CHANGER GOOFED;
REDERR=⊂'200⊃, comment NOT ENOUGH READINGS;
NOIERR=⊂'400⊃, comment POTS TOO NOISY;
SERERR=⊂'1000⊃, comment SERVO PROBLEMS;
LOKERR=⊂'2000⊃, comment LOOKUP FAILED;
NUMERR=⊂'4000⊃, comment ILLEGAL CAMERA NUMBER;
ADAERR=⊂'10000⊃; comment AD NOT AVAILABLE;
\ VARIABLES, DEFAULT VALUES;
SHORT REAL PAN,TILT,RANG,ZOOPOT,IPOT; \ calculated values for centering;
SHORT INTEGER MESS, \ message proc. number and misc.;
CAMNUM, \ last camera number used;
CAMLENS, \ last COHU lens number used;
CAMFLG, \ error bits (cleared only at start);
JOB; \ calling job name;
EXTERNAL SHORT REAL L1, L2, L3, \ final pot values for COHU servo;
L4, L5, L6, L7, L8, \ final pot values for Sierra servo;
P1, P2, P3, \ current pot values for COHU servo;
P4, P5, P6, P7, P8, \ current pot values for Sierra servo;
SREF, \ latest reference reading for Sierra;
CREF; \ latest reference reading for COHU;
EXTERNAL SHORT INTEGER LENS, \ current lens number from TSERVO;
STATUS, \ TSERVO status register;
TSERVO, \ COHU servo routine;
STATE, \ SSERVO status register;
SSERVO, \ Sierra servo routine;
POT, \ SSERVO command register;
IND, \ number of current color wheel;
TVCAM, FLINE, LLINE, RSIDE, LSIDE, TVWORD;
SAFEX REAL ARRAY MCOL,MICOL [1:3,1:3]; \ THIS IS THE CURRENT CAMERA XFORM;
SAFEX SHORT REAL ARRAY LCEN[1:3]; \ THIS IS THE CURRENT LENS CENTER ;
SAFEX REAL ARRAY MODEL[1:12,1:3]; \ THIS IS THE MODEL;
\ calibration constants;
SAFEX SHORT REAL ARRAY PPOT0,PPOTD, \ pan pot constants for internal model;
TPOT0, TPOTD, \ tilt pot constants for internal model;
FPOT0, FPOTD, \ focus pot constants for internal model;
MART, SWING, \ internal camera parameters;
GROREF, \ average reference voltage;
FOC, \ lens focal length;
FOCLEN0, FOCLENG[1:5], \ focus pot constants for depth calculations;
DP, \ relative lens center coordinates;
P0[1:5,1:3], \ pan-tilt axis intersection in table coords;
PP[1:5,1:2]; \ TV coords. of lens axis;
SHORT REAL C1, C2, \ zoom pot constants for focal length comp.;
ZPOT0, ZPOTD; \ zoom pot constants for internal model;
\ pot limits, error flags, and names - cosqr procedure;
preload_with 2020.0,1850.0,-2020.0, 154.0,
2030.0,2030.0,-1850.0,-2000.0,
2000.0,1800.0,-1720.0,-1425.0,
2048.0,1630.0,-2048.0, -790.0,
0000.0,1740.0, 0000.0,-1700.0;
safex short real array potlim[1:5,1:2,1:2];
preload_with PANERR,TILERR,FOCERR,ZOMERR,IRSERR;
safex short integer array poterr[1:5];
preload_with "PAN","TILT","FOCUS","ZOOM","IRIS";
safex string array potnam[1:5];
\ definitions for above array indicies;
DEFINE IPAN=⊂1⊃, ITILT=⊂2⊃, IFOCUS=⊂3⊃, IZOOM=⊂4⊃, IIRIS=⊂5⊃, COHU=⊂1⊃,
SIERRA=⊂2⊃, HIGH=⊂1⊃, LOW=⊂2⊃;
\ Solves the eq. Acos(x)+Bsin(x)+C=0 for x;
SIMPLE REAL PROCEDURE COSQR(REAL A,B,C);
BEGIN "COSQR"
REAL K,M,N,Y,Z;
K←A*C;
M←A↑2+B↑2;
N←B↑2-C↑2;
Y ← K/M;
Z ← SQRT(Y↑2+(N/M));
RETURN(ACOS(-Y+(IF B<0 THEN Z ELSE -Z)));
END "COSQR";
\ MISC. PROCEDURES
do not change or use defaults;
\ compute distance for given focus pot and cam/lens #;
SIMPLE REAL PROCEDURE FDIST(SHORT REAL FPOT; INTEGER LENS);
BEGIN "FDIST"
SHORT REAL FOCAL, OFFSET, SP;
SP ← FPOT*FOCLENG[LENS];
FOCAL ← FOC[LENS];
OFFSET ← FOCLEN0[LENS];
RETURN((FOCAL*(OFFSET+SP))/(OFFSET+SP-FOCAL));
END "FDIST";
\ compute focus pot reading for given distance and cam/lens #;
SIMPLE INTEGER PROCEDURE FPOT(SHORT REAL DIST; INTEGER LENS);
BEGIN "FPOT"
SHORT REAL O, F;
F ← FOC[LENS];
O ← FOCLEN0[LENS];
RETURN((DIST*(O-F)-F*O)/(FOCLENG[LENS]*(F-DIST))+.5);
END "FPOT";
\ recompute focus pot constants as function of zoom for Sierra;
SIMPLE PROCEDURE FOCZOM(SHORT REAL ZOOPOT);
BEGIN "FOCZOM"
FOC[5]←C1+ZOOPOT*C2;
FOCLEN0[5]←FPOT0[5]/KY+ZPOTD/((ZOOPOT-ZPOT0)*KY);
FOCLENG[5]←FPOTD[5]/KY;
END "FOCZOM";
\ check for pot out of bounds, set to closest limit
return TRUE if within limits - uses default camera;
SIMPLE BOOLEAN PROCEDURE POTCHK(REFERENCE REAL LL1; INTEGER INDEX);
BEGIN "POTCHK"
REAL HLIM, LLIM, HOLD;
HLIM ← POTLIM[INDEX,HIGH,CAMNUM];
LLIM ← POTLIM[INDEX,LOW,CAMNUM];
IF ¬(HLIM≤LL1≤LLIM) THEN
BEGIN "BADPOT"
HOLD ← LL1;
LL1 ← IF HLIM>LL1 THEN HLIM ELSE LLIM;
OUTSTR(POTNAM[INDEX]&"="&CVF(HOLD)&
"-OUT OF BOUNDS, SET TO "&CVF(LL1)CRLF);
ERROR(POTERR[INDEX]);
RETURN(FALSE);
END "BADPOT";
RETURN(TRUE);
END "POTCHK";
\ compute pot readings for each motor and check errors
uses default camera and given lens number
if outside limits, pot reading and value adjusted;
SHORT REAL NEWPAN, NEWTIL, NEWFOC, NEWZOM, NEWIRS; \ save values here;
SIMPLE PROCEDURE PANSET(REFERENCE SHORT REAL PAN; INTEGER L);
BEGIN "PANSET"
NEWPAN←(PAN-PPOT0[L])/PPOTD[L];
IF ¬POTCHK(NEWPAN,IPAN) THEN PAN←NEWPAN*PPOTD[L]+PPOT0[L];
END "PANSET";
SIMPLE PROCEDURE TILSET(REFERENCE SHORT REAL TILT; INTEGER L);
BEGIN "TILSET"
NEWTIL←(TILT-TPOT0[L])/TPOTD[L];
IF ¬POTCHK(NEWTIL,ITILT) THEN TILT←NEWTIL*TPOTD[L]+TPOT0[L];
END "TILSET";
SIMPLE PROCEDURE FOCSET(REFERENCE SHORT REAL RANG; INTEGER L);
BEGIN "FOCSET"
NEWFOC←FPOT(RANG,L);
IF ¬POTCHK(NEWFOC,IFOCUS) THEN RANG←FDIST(NEWFOC,L);
END "FOCSET";
SIMPLE PROCEDURE ZOMSET(REFERENCE SHORT REAL FOC_LGTH);
BEGIN "ZOMSET"
NEWZOM←(FOC_LGTH/25.4-C1)/C2;
IF ¬POTCHK(NEWZOM,IZOOM) THEN FOC_LGTH←(NEWFOC*C2+C1)*25.4;
FOCZOM(L7); \ fix this for focus computations;
END "ZOMSET";
\ READS DATA IN GILL'S FORMAT FOR GIVEN CAM/LENS #;
SIMPLE PROCEDURE DATXFR(INTEGER BLOCK);
BEGIN
BOOLEAN FLAG;
DEFINE XFR(X)=⊂ARRYIN(3,X,1)⊃,
DATASET= ⊂(IF CAMNUM=COHU THEN "DATA1[CAL,HE]"
ELSE "DATA2[CAL,HE]")⊃;
OPEN(3,"DSK",12,3,0,0,0,0);
LOOKUP(3,DATASET,FLAG);
IF FLAG THEN
BEGIN
OUTSTR("DATXFR: LOOKUP FAILED FOR "&DATASET CRLF);
ERROR(LOKERR);
RETURN;
END;
IF TYP_CAM THEN OUTSTR("DATXFR: RETRIEVING "&DATASET&CVS(BLOCK)CRLF);
USETI(3,1);
WORDIN(3);
USETI(3,BLOCK);
IF CAMNUM=SIERRA THEN
BEGIN BLOCK←5; XFR(ZPOT0); XFR(ZPOTD); END;
XFR(PPOT0[BLOCK]);
XFR(PPOTD[BLOCK]);
XFR(TPOT0[BLOCK]);
XFR(TPOTD[BLOCK]);
XFR(FPOT0[BLOCK]);
XFR(FPOTD[BLOCK]);
XFR(MART[BLOCK]);
XFR(SWING[BLOCK]);
XFR(|PP[BLOCK,1]|);
XFR(|PP[BLOCK,2]|);
XFR(|P0[BLOCK,1]|);
XFR(|P0[BLOCK,2]|);
XFR(|P0[BLOCK,3]|);
XFR(|DP[BLOCK,1]|);
XFR(|DP[BLOCK,2]|);
XFR(|DP[BLOCK,3]|);
XFR(FOC[BLOCK]);
XFR(FOCLEN0[BLOCK]);
XFR(FOCLENG[BLOCK]);
XFR(GROREF[BLOCK]);
IF CAMNUM=SIERRA THEN
BEGIN
WORDIN(3);
XFR(C1);
XFR(C2);
END;
RELEASE(3);
END "DATXFR";
\ SERVO THE DEFAULT CAMERA USING THE STORED POT VALUES;
SIMPLE PROCEDURE SERVO;
BEGIN
SHORT INTEGER I, EOF, STATSAV;
LABEL LOPEN, START, NOAD, NOSERV;
IF CAMNUM=COHU THEN
BEGIN "CINIT"
L1 ← NEWFOC/CREF;
L2 ← NEWTIL/CREF;
L3 ← NEWPAN/CREF;
END "CINIT" ELSE BEGIN "SINIT"
L4 ← NEWPAN/SREF;
L5 ← NEWTIL/SREF;
L6 ← NEWFOC/SREF;
L7 ← NEWZOM/SREF;
L8 ← NEWIRS/SREF;
END "SINIT";
STATSAV ← STATUS;
START: STATUS ← STATSAV;
IF CAMNUM=COHU THEN
BEGIN "CAM1SR"
SPWON(1,TSERVO);
WHILE ¬(STATUS LAND 1) DO;
SPWOFF;
IF (STATUS LAND '667777)='110000 THEN
NOAD: BEGIN "NOAD1"
OUTSTR("SERVO: ");
TELL(CVSIX("AD"));
RETYPE;
IF YES THEN GO START ELSE
BEGIN ERROR(ADAERR);RETURN;END;
END "NOAD1";
IF STATUS LAND '17700 THEN
BEGIN "SR1FAL"
OUTSTR("TSERVO FAILED:"&OCT(STATUS));
NOSERV: RETYPE;
IF YES THEN GO START ELSE
BEGIN ERROR(SERERR);RETURN;END;
END "SR1FAL";
IF DEB_CAM THEN OUTSTR("TSERVO:"&FLT(L1)&FLT(L2)&
FLT(L3)CRLF&FLT(P1)&FLT(P2)&FLT(P3)CRLF);
END "CAM1SR" ELSE
IF CAMNUM=SIERRA THEN
BEGIN "CAM2SR" INTEGER ARG; DEFINE SPCWGO=⊂'047000400003⊃;
I ← 20;
LOPEN: OPEN(1,"AD",'417,0,0,0,0,EOF←TRUE);
IF EOF THEN IF (I←I-1) THEN
BEGIN CALL(1,"SLEEP"); GO LOPEN; END ELSE GO NOAD;
STATE ← POT ← 0;
ARG ← '240000000000+LOCATION(SSERVO);
START_CODE MOVE 1,ARG; SPCWGO 1,; END;
WHILE ¬STATE DO;
SPWOFF;
IF STATE>1 THEN
BEGIN "SR2FAL"
OUTSTR("SSERVO FAILED: "&OCT(STATE));
GO NOSERV;
END "SR2FAL";
IF DEB_CAM THEN OUTSTR("TSERVO: "
&FLT(L4)&FLT(L5)&FLT(L6)&FLT(L7)&FLT(L8)CRLF
&FLT(P4)&FLT(P5)&FLT(P6)&FLT(P7)&FLT(P8)CRLF);
END "CAM2SR";
END "SERVO";
\ Compute the transformation matrices for the camera model
with given lens number and the last pot readings
Does not change defaults;
PROCEDURE PANTIL_CAM(SHORT INTEGER L;SHORT REAL PPOT,TPOT,FPOT,ZPOT;
SAFEX REAL ARRAY COL,ICOL,CENTER);
BEGIN SHORT INTEGER I,J;
REAL ACC,FMX,FMY,PAN,TILT, A, B, CC, D, A1, A2, A3, A4, A5;
SAFEX REAL ARRAY RP,RT,RPT,RS,R[1:3,1:3],C[1:3];
PAN ← PPOTD[L]*PPOT+PPOT0[L];
TILT ← TPOTD[L]*TPOT+TPOT0[L];
FMY ← FPOTD[L]*FPOT+FPOT0[L];
IF CAMNUM=SIERRA THEN FMY ← FMY+ZPOTD/(ZPOT-ZPOT0);
FMX ← FMY*MART[L];
RP[2,3] ← -1;
RPT[1,1] ← RP[1,1] ← RP[3,2] ← B ← -SIN(PAN);
RP[3,1] ← A ← -(RPT[1,2] ← RP[1,2] ← COS(PAN));
RT[1,1] ← RS[3,3] ← 1;
RPT[2,3] ← D ← -(RT[2,2] ← RT[3,3] ← COS(TILT));
R[3,3] ← RPT[3,3] ← RT[2,3] ← CC ← -(RT[3,2] ← SIN(TILT));
RPT[2,1] ← A3 ← CC*A;
RPT[2,2] ← A4 ← CC*B;
R[3,1] ← RPT[3,1] ← -D*A;
R[3,2] ← RPT[3,2] ← -D*B;
RS[1,1] ← RS[2,2] ← A1 ← COS(A2 ← SWING[L]);
RS[2,1] ← A5 ← -(RS[1,2] ← SIN(A2));
R[1,1] ← A1*B-A5*A3;
R[1,2] ← -A1*A-A5*A4;
R[1,3] ← -A5*D;
R[2,1] ← A5*B+A1*A3;
R[2,2] ← -A5*A+A1*A4;
R[2,3] ← A1*D;
A ← DP[L,1];
B ← DP[L,2];
CC ← DP[L,3];
FOR I←1 STEP 1 UNTIL 3 DO
C[I]←P0[L,I]+RPT[1,I]*A+RPT[2,I]*B+RPT[3,I]*CC;
\ -FMX/333 and PP[2]-PP[1]/333 are corrections for
the non-orthogonality of the TV scan axes;
FOR I←1 STEP 1 UNTIL 3 DO
BEGIN
COL[I,1]←R[I,1];
COL[I,2]←R[I,2];
ACC←0;
FOR J←1 STEP 1 UNTIL 3 DO ACC←ACC-R[I,J]*C[J];
COL[I,3]←ACC;
END;
A ← FMX/333.0;
B ← PP[L,2]-(CC←PP[L,1])/333.0;
FOR J←1 STEP 1 UNTIL 3 DO
BEGIN
COL[2,J]←-A*COL[1,J]+FMY*COL[2,J]+B*COL[3,J];
COL[1,J]←FMX*COL[1,J]+CC*COL[3,J];
END;
INVRT(COL,ICOL);
ARRTRAN(CENTER,C);
END "PANTIL_CAM";
\ CREATE UPDATED TRANSFORM;
SIMPLE PROCEDURE NEWTRAN(INTEGER LENS; SHORT REAL PANPOT, TILPOT, FOCPOT,
ZOOPOT, IPOT, REF);
BEGIN "NEWTRA"
SHORT REAL CAMPAN, CAMTIL, CAMRANG, FOCAL;
PANTIL_CAM(LENS,PANPOT,TILPOT,FOCPOT,ZOOPOT,MCOL,MICOL,LCEN);
IF CAMNUM=SIERRA THEN FOCZOM(ZOOPOT);
ARRBLT (MODEL[1,1],MCOL[1,1],9);
ARRBLT (MODEL[4,1],LCEN[1],3);
MODEL[5,1] ← PP[LENS,1];
MODEL[5,2] ← PP[LENS,2];
MODEL[5,3] ← 1.0;
ARRBLT (MODEL[6,1],MICOL[1,1],9);
MODEL[9,1] ← CAMPAN ← (PPOTD[LENS]*PANPOT+PPOT0[LENS])*DEG;
MODEL[9,2] ← CAMTIL ← (TPOTD[LENS]*TILPOT+TPOT0[LENS])*DEG;
MODEL[9,3] ← CAMRANG ← FDIST(FOCPOT,LENS);
MODEL[10,1] ← CAMNUM;
MODEL[10,2] ← FOCAL ← FOC[LENS]*25.4;
MODEL[10,3] ← CAMFLG;
MODEL[11,1] ← PANPOT/REF;
MODEL[11,2] ← TILPOT/REF;
MODEL[11,3] ← FOCPOT/REF;
MODEL[12,1] ← IF CAMNUM = COHU THEN LENS ELSE ZOOPOT/REF;
MODEL[12,2] ← IPOT/REF;
MODEL[12,3] ← REF;
IF DEB_CAM THEN OUTSTR("CAM_UPDATE: "&INT(CAMNUM)&INT(LENS)&
(IF CAMNUM=COHU THEN INT(CAMLENS) ELSE NULL) CRLF&
FLT(CAMPAN)&" DEGREES"CRLF&
FLT(CAMTIL)&" DEGREES"CRLF&
FLT(CAMRANG)&" INCHES"CRLF&
FLT(IPOT)&" UNITS"CRLF&
FLT(FOCAL)&" MM"CRLF);
END "NEWTRA";
\ UPDATE THE TRANSFORM FOR THE GIVEN CAMERA
if camera 1, sets default lens to lens in position;
SIMPLE MESSAGE PROCEDURE CAM_UPDATE(INTEGER CAMNO);
BEGIN LABEL ETA, NOAD;
SHORT REAL IND, SIND, \ # of readings and sqrt;
SFOC, STIL, SPAN, SZOM, SIRIS, \ sums of pot readings;
FMAX, TMAX, PMAX, ZMAX, IMAX, \ maximum readings;
FMIN, TMIN, PMIN, ZMIN, IMIN, \ minimum readings;
DIFFOC, DIFTIL, DIFPAN, DIFZOM, DIFIRIS, \ total differences;
DP, DT, DF, DZ, DI, \ ave. differences;
TILPOT, FOCPOT, PANPOT, \ ave. pot readings;
REF; \ reference voltage;
SHORT INTEGER I, L;
CAMSET(CAMNO); \ initialize variables;
ETA: SFOC ← SPAN ← STIL ← SZOM ← SIRIS ← 0;
FMAX ← TMAX ← PMAX ← ZMAX ← IMAX ← -10000;
FMIN ← TMIN ← PMIN ← ZMIN ← IMIN ← 10000;
IF CAMNUM=COHU THEN \ get COHU readings;
BEGIN "CM1UPD"
IMAX ← IMIN ← 0; \ no iris for COHU;
STATUS←1;
SPWON(1,TSERVO);
I ← '44;
FOR IND←0 STEP 1 UNTIL 19 DO
BEGIN "CM1LOP"
STATUS←I;
I ← 4;
WHILE ¬(STATUS LAND 1) DO;
IF (STATUS LAND '667777)='110000 THEN
NOAD: BEGIN "NOAD2"
SPWOFF;
OUTSTR("SERVO: ");
TELL(CVSIX("AD"));
RETYPE;
IF YES THEN GO ETA ELSE
BEGIN ERROR(ADAERR);RETURN;END;
END "NOAD2";
IF STATUS LAND '17700 THEN
BEGIN "SR3FAL"
OUTSTR("TSERVO FAILED:"&OCT(STATUS)CRLF);
CONTINUE;
END "SR3FAL";
SFOC←SFOC+P1; \ update sums and minmax;
STIL←STIL+P2;
SPAN←SPAN+P3;
IF P1>FMAX THEN FMAX←P1;
IF P1<FMIN THEN FMIN←P1;
IF P2>TMAX THEN TMAX←P2;
IF P2<TMIN THEN TMIN←P2;
\ UPDATE THE TRANSFORM (cont.);
IF P3>PMAX THEN PMAX←P3;
IF P3<PMIN THEN PMIN←P3;
END "CM1LOP";
SPWOFF;
L ← CAMLENS ← LENS+1; \ set default lens for COHU;
END "CM1UPD" ELSE
IF CAMNUM=SIERRA THEN \ get Sierra readings;
BEGIN "CM2UPD"
FOR IND←0 STEP 1 UNTIL 19 DO
BEGIN "CM2LOP"
STATE ← 0;
POT ← 1;
SPWON(0,SSERVO);
WHILE ¬STATE DO;
SPWOFF;
IF STATE LAND '100 THEN GO NOAD;
IF ¬(STATE=1) THEN
BEGIN "SR4FAL"
OUTSTR("SSERVO FAILED:"&OCT(STATE)CRLF);
CONTINUE;
END "SR4FAL";
SPAN←SPAN+P4; \ update sums and minmax;
SFOC←SFOC+P6;
STIL←STIL+P5;
SZOM←SZOM+P7;
SIRIS←SIRIS+P8;
IF P4>PMAX THEN PMAX←P4;
IF P4<PMIN THEN PMIN←P4;
IF P5>TMAX THEN TMAX←P5;
IF P5<TMIN THEN TMIN←P5;
IF P6>FMAX THEN FMAX←P6;
IF P6<FMIN THEN FMIN←P6;
IF P7>ZMAX THEN ZMAX←P7;
IF P7<ZMIN THEN ZMIN←P7;
IF P8>IMAX THEN IMAX←P8;
IF P8<IMIN THEN IMIN←P8;
END "CM2LOP";
L ← 5; \ set cam/lens #;
END "CM2UPD";
IF IND<10 THEN \ check for good read;
BEGIN "RDLOW"
OUTSTR("CAM_UPDATE: NOT ENOUGH READINGS"&INT(IND));
RETYPE;
IF YES THEN GO ETA ELSE ERROR(REDERR);
END "RDLOW";
REF← IF CAMNUM=COHU THEN CREF ELSE SREF;
FOCPOT←SFOC*REF/IND; \ get average readings;
TILPOT←STIL*REF/IND;
PANPOT←SPAN*REF/IND;
\ UPDATE THE TRANSFORM - process readings;
IPOT ← SIRIS*REF/IND;
IF CAMNUM=SIERRA THEN L7←ZOOPOT←SZOM*REF/IND;
DIFFOC←(FMAX-FMIN)*REF; \ maximum differences;
DIFTIL←(TMAX-TMIN)*REF;
DIFPAN←(PMAX-PMIN)*REF;
DIFIRIS←(IMAX-IMIN)*REF;
IF CAMNUM=SIERRA THEN DIFZOM←(ZMAX-ZMIN)*REF;
IF DEB_CAM THEN OUTSTR("CAM_UPDATE:"
&FLT(FOCPOT)&FLT(TILPOT)&FLT(PANPOT)&FLT(IPOT)
&(IF CAMNO=2 THEN FLT(ZOOPOT) ELSE NULL) CRLF
&INT(DIFFOC)&INT(DIFTIL)&INT(DIFPAN)
&INT(DIFIRIS) CRLF
&(IF CAMNO=2 THEN INT(DIFZOM) ELSE NULL) CRLF);
SIND←4*SQRT(IND);
DZ ← IF CAMNUM=SIERRA THEN DIFZOM/SIND ELSE 0;
DF←DIFFOC/SIND; \ ave. differences;
DT←DIFTIL/SIND;
DP←DIFPAN/SIND;
DI←DIFIRIS/SIND;
IF DI>.75∨DP>.75∨DT>.75∨DF>1∨(CAMNO=2∧DZ>1) THEN
BEGIN "DIFBAD"
OUTSTR("CAM_UPDATE: POTS TOO NOISY "&INT(CAMNUM));
IF DF>1.0 THEN OUTSTR(INT(DIFFOC));
IF DT>.75 THEN OUTSTR(INT(DIFTIL));
IF DP>.75 THEN OUTSTR(INT(DIFPAN));
IF DI>.75 THEN OUTSTR(INT(DIFIRIS));
IF DZ>1.0 THEN OUTSTR(INT(DIFZOM));
OUTSTR(NULL CRLF);
ERROR(NOIERR);
END "DIFBAD";
IF CAMFLG LAND (ADAERR+REDERR+NOIERR) THEN
BEGIN "ERROR"
RETYPE;
IF YES THEN
BEGIN "RETRY"
CAMFLG←CAMFLG LAND LNOT (ADAERR+REDERR+NOIERR);
GOTO ETA
END "RETRY" ELSE
IF CAMFLG LAND ADAERR THEN
BEGIN "ADERR"
OUTSTR("CAM_UPDATE: CAMERA_MODEL NOT UPDATED"CRLF);
SAVE;
RETURN;
END "ADERR";
END "ERROR";
NEWTRAN(L,PANPOT,TILPOT,FOCPOT,ZOOPOT,IPOT,REF);
ARRTRAN(GLOBAL DATUM(CURCAM[CAMNUM]),MODEL);
SAVE;
END "CAM_UPDATE";
\ move camera - default camera set to given camera and update
This is the main camera moving routine
BITS tells what is to be done with bits referencing the elements
of VAL whose values should be used to change the camera state
BIT INDEX CONTENTS OF VAL IS NEW VALUE OF
1 1 pan setting in radians
2 2 tilt setting in radians
4 3 focus setting in inches
'10 4 zoom setting in mm. (camera 2 only)
'20 5 iris setting in pot units (camera 2 only)
'40 6 lens number wanted (camera 1 only)
'100 7 color wheel index wanted ;
SIMPLE MESSAGE PROCEDURE CHNG_CAM(INTEGER CAMNO,BITS; SAFEX REAL ARRAY VAL);
BEGIN "CHNG_CAM"
SHORT REAL REF;
INTEGER L, HOLD, STAT, I;
CAMFLG ← 0;
CAMSET(CAMNO);
IF CAMNUM=COHU THEN
BEGIN "COHU"
IF BITS LAND '40 THEN
BEGIN "LENS"
L ← VAL[6];
CALLEN;
IF 1≤L≤4 THEN
BEGIN "LENOK"
HOLD←STAT←IF LENS≠L-1 THEN '20 ELSE '10;
LENS ← L-1;
END "LENOK" ELSE BEGIN "NOLENS"
OUTSTR("LENS OUT OF BOUNDS -"&CVS(L)CRLF);
ERROR(LENERR);
L ← LENS+1;
HOLD ← STAT ← '10;
END "NOLENS";
END "LENS";
END "COHU" ELSE BEGIN "SIERRA"
L ← 5;
STAT ← HOLD ← 0;
IF BITS LAND '20 THEN
BEGIN "IRIS"
NEWIRS←VAL[5];
POTCHK(NEWIRS,IIRIS);
END "IRIS" ELSE NEWIRS ← P8*SREF;
IF BITS LAND '10 THEN ZOMSET(VAL[4]) ELSE NEWZOM←P7*SREF;
END "SIERRA";
REF ← IF L=5 THEN SREF ELSE CREF;
IF BITS LAND '100 THEN
BEGIN "COLOR"
CWHEEL(VAL[7]);
COLFILT_ACC ← IND;
IF VAL[7]<6 THEN
BEGIN "DELAY"
I ← 10000;
WHILE I DO I←I-1;
END "DELAY"
END "COLOR";
IF BITS LAND 1 THEN PANSET(VAL[1],L) ELSE
NEWPAN ← (IF CAMNUM=COHU THEN P3 ELSE P4)*REF;
IF BITS LAND 2 THEN TILSET(VAL[2],L) ELSE
NEWTIL ← (IF CAMNUM=COHU THEN P2 ELSE P5)*REF;
IF BITS LAND 4 THEN FOCSET(VAL[3],L) ELSE
NEWFOC ← (IF CAMNUM=COHU THEN P1 ELSE P6)*REF;
STATUS←STAT;
SERVO;
CAM_UPDATE(CAMNUM);
IF HOLD='20∧LENS≠L-1 THEN
BEGIN "GOOF"
OUTSTR("I GOOFED, NOW YOU HAVE LENS NO. "&CVS(LENS+1)CRLF);
ERROR(CHGERR);
SAVE;
END "GOOF";
END "CHNG_CAM";
\ CALCULATIONS COMMON TO PREDICTION AND CENTERING GIVEN LENS
given lens number (or 0 for Sierra);
SIMPLE PROCEDURE CALC(INTEGER RLENS; SHORT REAL XTC,YTC,ZTC;
REFERENCE REAL ZOOM);
BEGIN "CALC"
SHORT REAL XCC,YCC,ZCC,D;
IF ¬RLENS THEN
BEGIN
IF ZOOM THEN ZOMSET(ZOOM) ELSE ZOOM←(ZOOPOT*C2+C1)*25.4;
RLENS ← 5;
CAMNUM ← SIERRA;
END ELSE CAMNUM ← COHU;
XCC ← P0[RLENS,1]-XTC;
YCC ← P0[RLENS,2]-YTC;
ZCC ← P0[RLENS,3]-ZTC;
PAN ← COSQR(YCC,-XCC,DP[RLENS,1]);
IF RLENS=5∧(P0[RLENS,2]+DP[RLENS,1]-YTC<0) THEN PAN ← -PAN;
PANSET(PAN,RLENS);
D ← XCC*COS(PAN)+YCC*SIN(PAN);
TILT ← COSQR(ZCC,-D,-DP[RLENS,2]);
TILSET(TILT,RLENS);
RANG ← D*COS(TILT)+ZCC*SIN(TILT)-DP[RLENS,3];
FOCSET(RANG,RLENS);
END;
\ CENTER CAMERA - sets default camera to given and update;
MESSAGE PROCEDURE CAM_CENTER(INTEGER RLENS; SHORT REAL XTC,YTC,ZTC,ZOOM);
BEGIN "CAM_CEN"
PRELOAD_WITH [7] 0;
SAFEX OWN REAL ARRAY VAL[1:7];
CAMFLG ← 0;
IF ¬(0≤RLENS≤4) THEN BEGIN ERROR(NUMERR); SAVE; RETURN; END;
CALC(RLENS, XTC, YTC, ZTC, ZOOM);
IF CAMFLG THEN BEGIN SAVE; RETURN; END;
VAL[1] ← PAN;
VAL[2] ← TILT;
VAL[3] ← RANG;
VAL[4] ← ZOOM;
VAL[6] ← RLENS;
CHNG_CAM(2-(RLENS>0),'57,VAL);
ifc debug thenc
IF DEB_CAM THEN
BEGIN "DEBUG"
REAL X, Y;
SAFE INTEGER ARRAY BUF[1:100];
CONVRT(PP[CAMLENS,1],PP[CAMLENS,2],X,Y);
OUTSTR(CVF(X)&","&CVF(Y) CRLF);
TVWORD ← GIOWD(BUF);
TVCAM ← CAMNUM;
LSIDE ← PP[CAMLENS,1];
FLINE ← PP[CAMLENS,2];
RSIDE ← LSIDE+10;
LLINE ← FLINE+10;
WHILE INCHRS<0 DO TVIN;
END "DEBUG";
endc;
END "CAM_CEN";
\ TRANSFORM PREDICTION - set default camera to given camera
ignores default lens;
SIMPLE MESSAGE PROCEDURE CAM_PRED(INTEGER RLENS; SHORT REAL XTC,
YTC,ZTC,ZOOM);
BEGIN
INTEGER L;
CAMFLG ← 0;
L←IF ¬RLENS THEN SIERRA ELSE IF 1≤RLENS≤4 THEN COHU ELSE 0;
CAMSET(L);
CALC(RLENS,XTC,YTC,ZTC,ZOOM);
IF CAMFLG THEN BEGIN SAVE; RETURN; END;
NEWTRAN(RLENS,NEWPAN,NEWTIL,NEWFOC,NEWZOM,IPOT,IF CAMNUM=COHU
THEN CREF ELSE SREF);
IF CAMNUM=SIERRA THEN FOCZOM(ZOOPOT);
ARRTRAN(GLOBAL DATUM(PREDCM[CAMNUM]),MODEL);
SAVE;
END "CAM_PRED";
\ initialize program - set default camera to given camera;
SIMPLE MESSAGE PROCEDURE CAM_INIT(INTEGER CAMNO);
BEGIN
INTEGER BLOCK;
CAMSET(CAMNO);
IF CAMNUM=COHU THEN
FOR BLOCK←1 STEP 1 UNTIL 4 DO DATXFR(BLOCK) ELSE DATXFR(1);
SAVE;
END "CAM_INIT";
\ PRINT LIMITS FOR GIVEN CAMERA
does not change or use defaults;
ifc debug thenc
SIMPLE PROCEDURE LIMPRT(SHORT INTEGER CAMNUM);
BEGIN
SHORT INTEGER CAMLENS,I, J, K;
SHORT REAL VAR1, VAR2, CON1, CON2;
OUTSTR(NULL CRLF&"CAMERA "&CVS(CAMNUM)&" :"CRLF);
J ← IF CAMNUM=COHU THEN 1 ELSE 5;
I ← IF CAMNUM=COHU THEN 4 ELSE 5;
FOR CAMLENS←J STEP 1 UNTIL I DO
BEGIN "LENS"
OUTSTR(" LENS "&CVS(CAMLENS)&" :"CRLF);
VAR1 ← FDIST(POTLIM[IFOCUS,HIGH,CAMNUM],CAMLENS);
VAR2 ← FDIST(POTLIM[IFOCUS,LOW,CAMNUM],CAMLENS);
OUTSTR(" FOCUS RANGE IS "&CVF(VAR2)&" TO "&CVF(VAR1)&
" INCHES"CRLF);
CON1 ← PPOTD[CAMLENS];
CON2 ← PPOT0[CAMLENS];
VAR1 ← (CON1*POTLIM[IPAN,HIGH,CAMNUM]+CON2)*DEG;
VAR2 ← (CON1*POTLIM[IPAN,LOW,CAMNUM]+CON2)*DEG;
OUTSTR(" PAN RANGE IS "&CVF(VAR2)&" TO "&CVF(VAR1)&
" DEGREES"CRLF);
CON1 ← TPOTD[CAMLENS];
CON2 ← TPOT0[CAMLENS];
VAR1 ← (CON1*POTLIM[ITILT,HIGH,CAMNUM]+CON2)*DEG;
VAR2 ← (CON1*POTLIM[ITILT,LOW,CAMNUM]+CON2)*DEG;
IF CAMNUM=COHU THEN VAR1↔VAR2;
OUTSTR(" TILT RANGE IS "&CVF(VAR1)&" TO "&CVF(VAR2)&
" DEGREES"CRLF);
IF CAMNUM=SIERRA THEN
BEGIN "ZOOM"
OUTSTR(" ZOOM RANGE IS ");
OUTSTR(CVF((C1*POTLIM[IZOOM,HIGH,SIERRA]+C2)*25.4));
OUTSTR(" TO "&CVF((C1*POTLIM[IZOOM,LOW,SIERRA]+C2)
*25.4)&" MM" CRLF);
END "ZOOM";
END "LENS";
END "LIMPRT";
\ COORDINATE CONVERSION ROUTINES;
\ convert given TV coords. to table coords. with current transform;
SIMPLE PROCEDURE CONVRT(REAL XI, YI; REFERENCE REAL X, Y);
BEGIN REAL Z;
X ← MICOL[1,1]*XI + MICOL[1,2]*YI + MICOL[1,3];
Y ← MICOL[2,1]*XI + MICOL[2,2]*YI + MICOL[2,3];
Z ← MICOL[3,1]*XI + MICOL[3,2]*YI + MICOL[3,3];
X ← X/Z;
Y ← Y/Z;
END;
\ give table coords. of point marked with cursor.
sets default camera to given camera and updates;
PROCEDURE SHOW(INTEGER CAMNO);
BEGIN
SAFE INTEGER ARRAY BUF[1:100];
SHORT INTEGER I, INC;
SHORT REAL X, Y;
CAM_UPDATE(CAMNUM);
TVWORD ← GIOWD(BUF);
TVCAM ← CAMNUM;
OUTSTR("set upper left corner of cursor:" CRLF&
" ←→↑↓ move cursor in indicated direction" crlf&
" ↔ switches increment between fine and coarse" crlf&
" M will mark point and type table coords." crlf&
" X will leave routine" CRLF);
LSIDE ← 160;
FLINE ← 125;
INC ← 16;
WHILE TRUE DO
BEGIN "MARK"
LLINE ← FLINE+10;
RSIDE ← LSIDE+10;
DO TVIN UNTIL (I←INCHRS)>0;
IF I="↔" THEN INC ← IF INC=16 THEN 1 ELSE 16 ELSE
IF I="←" THEN LSIDE ← LSIDE-INC MAX 15 ELSE
IF I="→" THEN LSIDE ← LSIDE+INC MIN 315 ELSE
IF I="↑" THEN FLINE ← FLINE-INC MAX 15 ELSE
IF I="↓" THEN FLINE ← FLINE+INC MIN 245 ELSE
IF I="X" THEN DONE ELSE
IF I="M" THEN
BEGIN
CONVRT(LSIDE,FLINE,X,Y);
OUTSTR(CVF(X)&","&CVF(Y) CRLF);
END;
END "MARK";
END;
\ PROGRAM TEST ROUTINE;
INTEGER FLAGS;
PRELOAD_WITH [7] 0;
SAFEX REAL ARRAY VAL[1:7];
SIMPLE INTEGER PROCEDURE IIN(STRING NAME; INTEGER FLAG);
BEGIN
STRING ST;
OUTSTR(NAME&" IS ");
ST ← INCHWL;
IF LENGTH(ST) THEN FLAGS ← FLAGS LOR FLAG;
RETURN(CVD(ST));
END;
SIMPLE REAL PROCEDURE RIN(STRING NAME; INTEGER FLAG);
BEGIN
INTEGER I;
STRING ST;
OUTSTR(NAME&" IS ");
ST ← INCHWL;
IF LENGTH(ST) THEN FLAGS ← FLAGS LOR FLAG;
RETURN(REALSCAN(ST,I));
END;
SIMPLE INTEGER PROCEDURE CAMERA;
BEGIN INTEGER I;
I ← IIN("CAMERA #",0);
RETURN(IF I THEN I ELSE CAMNUM);
END;
SIMPLE PROCEDURE TESTIT;
WHILE TRUE DO
BEGIN "TEST"
INTEGER REQUEST, I;
OUTSTR("COMMANDS ARE:"CRLF&
"0-EXIT"CRLF&
"1-change camera state"CRLF&
"2-center a camera on a point"CRLF&
"3-complement debug flag"CRLF&
"4-update readings for a camera"CRLF&
"5-print limits for a camera"CRLF&
"6-convert TV coords. for a camera" CRLF);
REQUEST ← IIN("COMMAND NUMBER ",0);
IF ¬(0≤REQUEST≤6) THEN
BEGIN
OUTSTR("ILLEGAL COMMAND"CRLF);
CONTINUE;
END;
FLAGS ← CAMFLG ← 0;
CASE REQUEST OF
BEGIN
RETURN;
BEGIN "CHANGE"
I ← CAMERA;
VAL[1] ← RIN("PAN (DEGREES)",1)/DEG;
VAL[2] ← RIN("TILT (DEGREES)",2)/DEG;
VAL[3] ← RIN("FOCUS (INCHES)",4)/DEG;
IF I=SIERRA THEN
BEGIN
VAL[4] ← RIN("ZOOM (MM)",'10);
VAL[5] ← RIN("IRIS (POT UNITS)",'20);
END ELSE VAL[6]← IIN("LENS #",'40);
VAL[6] ← IIN("COLOR #",'100);
CHNG_CAM(I,FLAGS,VAL);
END "CHANGE";
BEGIN "CENTER"
I ← IIN("LENS # (0 FOR SIERRA)",0);
CAM_CENTER(I,RIN("X",0),RIN("Y",0),RIN("Z",0),
IF ¬I THEN RIN("ZOOM (MM)",0) ELSE 0);
END "CENTER";
BEGIN "PREDIC" INTEGER SAV;
SAV ← DEB_CAM;
DEB_CAM ← TRUE;
I ← IIN("LENS # (0 FOR SIERRA)",0);
CAM_PRED(I,RIN("X",0),RIN("Y",0),RIN("Z",0),
IF ¬I THEN RIN("ZOOM (MM)",0) ELSE 0);
DEB_CAM ← SAV;
END "PREDIC";
DEB_CAM←¬DEB_CAM;
CAM_UPDATE(CAMERA);
LIMPRT(CAMERA);
SHOW(CAMERA);
END;
OUTSTR("ERROR BITS ARE "&CVOS(CAMFLG) CRLF CRLF);
END "TEST";
endc;
\ THIS IS THE MAIN PROGRAM;
CAMFLG ← 0;
TYP_CAM←TRUE;
YES_CAM←TRUE;
JOB ← CVSIX("TTY");
SETFORMAT(7,2);
FOR MESS ← 1,2 DO
BEGIN
CAMERR[MESS] ← 0;
CAMJOB[MESS] ← JOB;
CURCAM[MESS] ← GLOBAL NEW(MODEL);
PREDCM[MESS] ← GLOBAL NEW(MODEL);
END;
FOR CAMNUM←1 STEP 1 UNTIL 2 DO
BEGIN
CAM_INIT(CAMNUM);
CAM_UPDATE(CAMNUM);
END;
ifc debug thenc
IF RUN=0 THEN
BEGIN
OUTSTR("...TYPE Y FOR TEST MODE:"CRLF);
IF YES THEN TESTIT;
END ;
endc;
PUT_DATA(0,0,"CAM");
OUTSTR("CAM-ACTIVATED"CRLF);
WHILE TRUE DO
BEGIN
MESS ← GET_ENTRY ('120,NULL,"CAM",NULL);
CAMFLG←0;
JOB ← CVSIX(GET_DATA(1,MESS));
MESS←QUEUE('600,MESS);
END;
END "CAMERA";